/*
 * Copyright (c) 2022 Huawei Device Co., Ltd.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

import java.util.HashMap;
import java.util.Map;

class Px {
    public Double[] V;
    Px(Double X, Double Y, Double Z){
        this.V = new Double[]{X,Y,Z,1.0};
    }
}

class QType {
    public Px[] arr = new Px[9];
    public double LastPx = 0;
    public double Lastx = 0;
    public double Lasty = 0;
    public Double[][] Normal = new Double[][]{};;
    public boolean[] Line = new boolean[]{};;
    public int[][] Edge = new int[][]{};;
    Double NumPx;
    QType() {}
}

class Testing {
    public static int LoopCount = 0;
    public static int LoopMax = 0;
}

 class ThreeDCube {
    public static QType Q = new QType();
    public static Double[][] MTrans = new Double[][]{};
    public static Double[][] MQube = new Double[][]{};
    public static Double[][] I = new Double[][]{};
    public static Double[] Origin = new Double[]{};
    public static Map<Integer, Double> validation = new HashMap<>(){{
        put(20, 2889.0000000000045);
        put(40, 2889.0000000000055);
        put(80, 2889.000000000005);
        put(160, 2889.0000000000055);
    }};
    public static void DrawLine(Px From, Px To) {
        double x1 = From.V[0];
        double x2 = To.V[0];
        double y1 = From.V[1];
        double y2 = To.V[1];
        double dx = Math.abs(x2 - x1);
        double dy = Math.abs(y2 - y1);
        double x = x1;
        double y = y1;
        int IncX1;
        int IncY1;
        int IncX2;
        int IncY2;
        int Den;
        int Num;
        int NumAdd;
        int NumPix;

        if (x2 >= x1) {
            IncX1 = 1;
            IncX2 = 1;
        } else {
            IncX1 = -1;
            IncX2 = -1;
        }
        if (y2 >= y1) {
            IncY1 = 1;
            IncY2 = 1;
        } else {
            IncY1 = -1;
            IncY2 = -1;
        }
        if (dx >= dy) {
            IncX1 = 0;
            IncY2 = 0;
            Den = (int)dx;
            Num = (int)(dx / 2);
            NumAdd = (int)dy;
            NumPix = (int)dx;
        } else {
            IncX2 = 0;
            IncY1 = 0;
            Den = (int)dy;
            Num = (int)(dy / 2);
            NumAdd = (int)dx;
            NumPix = (int)dy;
        }

        NumPix = (int)Math.round(Q.LastPx + NumPix);

        double i = Q.LastPx;
        for (; i < NumPix; i++) {
            Num += NumAdd;
            if (Num >= Den) {
                Num -= Den;
                x += IncX1;
                y += IncY1;
            }
            x += IncX2;
            y += IncY2;
        }
        Q.Lastx = x;
        Q.Lasty = y;
        Q.LastPx = NumPix;
    }

    public static DoubleFunction<Double[] , Double[] , Double[]> CalcCross = (Double[] V0, Double[] V1) -> {
         Double[] Cross = new Double[]{0.0, 0.0, 0.0, 0.0};
         Cross[0] = V0[1] * V1[2] - V0[2] * V1[1];
         Cross[1] = V0[2] * V1[0] - V0[0] * V1[2];
         Cross[2] = V0[0] * V1[1] - V0[1] * V1[0];
         return Cross;
    };

    public static ThreeDoubleIDoubleFunction<Double[], Double[], Double[], Double[]> CalcNormal = (Double[] V0, Double[] V1, Double[] V2) -> {
        Double[] A = new Double[]{0.0, 0.0, 0.0, 0.0};
        Double[] B = new Double[]{0.0, 0.0, 0.0, 0.0};
        for (int i = 0; i < 3; i++) {
            A[i] = V0[i] - V1[i];
            B[i] = V2[i] - V1[i];
        }
        A = CalcCross.apply(A, B);

        Double x = A[0] * A[0] + A[1] * A[1] + A[2] * A[2];
        Double Length = Math.sqrt(x);
        for (int i = 0; i < 3; i++) {
            A[i] =  (A[i] / Length);
        }
        A[3] = 1.0;
        return A;
    };

    // multiplies two matrices
    public static DoubleFunction<Double[][] , Double[][] , Double[][]> MMulti = (Double[][] M1, Double[][] M2) -> {
        Double[][] M = new Double[4][4];
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                M[i][j] =
                        M1[i][0] * M2[0][j] + M1[i][1] * M2[1][j] + M1[i][2] * M2[2][j] + M1[i][3] * M2[3][j];
            }
        }
        return M;
    };

    //multiplies matrix with vector
    public static DoubleFunction<Double[][] , Double[] , Double[]> VMulti = (Double[][] M, Double[] V) -> {
        Double[] Vect = new Double[4];
        for (int i = 0; i < 4; i++) {
            Vect[i] = M[i][0] * V[0] + M[i][1] * V[1] + M[i][2] * V[2] + M[i][3] * V[3];
        }
        return Vect;
    };

    public static DoubleFunction<Double[][] , Double[] , Double[]> VMulti2 = (Double[][] M, Double[] V) -> {
         Double[] Vect = new Double[4];
         for (int i = 0; i < 4; i++) {
             Vect[i] = M[i][0] * V[0] + M[i][1] * V[1] + M[i][2] * V[2];
         }
         return Vect;
    };

    // add to matrices
    public static DoubleFunction<Double[][] , Double[][] , Double[][]> MAdd = (Double[][] M1, Double[][] M2) -> {
        Double[][] M = new Double[4][4];
        int i = 0;
        int j = 0;
        for (; i < 4; i++) {
            j = 0;
            for (; j < 4; j++) { M[i][j] = M1[i][j] + M2[i][j];}
        }
        return M;
    };

    public static Function<Double[][], Double, Double, Double, Double[][]> Translate = (Double[][] M, Double Dx, Double Dy, Double Dz) -> {
         Double[][] T = {
                 {1.0, 0.0, 0.0, Dx},
                 {0.0, 1.0, 0.0, Dy},
                 {0.0, 0.0, 1.0, Dz},
                 {0.0, 0.0, 0.0, 1.0}
         };
         return MMulti.apply(T, M);
    };

    public static DoubleFunction<Double[][], Double, Double[][]> RotateX = (Double[][] M, Double Phi) ->{
         double a = Phi;
         a *= Math.PI / 180;
         double cos = Math.cos(a);
         double sin = Math.sin(a);
         Double[][] R = {
                 {1.0, 0.0, 0.0, 0.0},
                 {0.0, cos, -sin, 0.0},
                 {0.0, sin, cos, 0.0},
                 {0.0, 0.0, 0.0, 1.0}
         };
         return MMulti.apply(R, M);
    };

    public static DoubleFunction<Double[][], Double, Double[][]> RotateY = (Double[][] M, Double Phi) ->{
         double a = Phi;
         a *= Math.PI / 180;
         double cos = Math.cos(a);
         double sin = Math.sin(a);
         Double[][] R = {
                 {cos, 0.0, sin, 0.0},
                 {0.0, 1.0, 0.0, 0.0},
                 {-sin, 0.0, cos, 0.0},
                 {0.0, 0.0, 0.0, 1.0}
         };
         return MMulti.apply(R, M);
    };

    public static DoubleFunction<Double[][], Double, Double[][]> RotateZ = (Double[][] M, Double Phi) ->{
         double a = Phi;
         a *= Math.PI / 180;
         double cos = Math.cos(a);
         double sin = Math.sin(a);
         Double[][] R = {
                 {cos, -sin, 0.0, 0.0},
                 {sin, cos, 0.0, 0.0},
                 {0.0, 0.0, 1.0, 0.0},
                 {0.0, 0.0, 0.0, 1.0}
         };
         return MMulti.apply(R, M);
    };


    public static VoidINullSupplier DrawQube = () ->{
        // calc current normals
        Double[][] CurN = new Double[6][];
        int i = 5;
        Q.LastPx = 0;
        for (; i > -1; i--) {
            CurN[i] = VMulti2.apply(MQube, Q.Normal[i]);
        }
        if (CurN[0][2] < 0) {
            if (!Q.Line[0]) { DrawLine(Q.arr[0], Q.arr[1]); Q.Line[0] = true;}

            if (!Q.Line[1]) { DrawLine(Q.arr[1], Q.arr[2]); Q.Line[1] = true;}

            if (!Q.Line[2]) { DrawLine(Q.arr[2], Q.arr[3]); Q.Line[2] = true;}

            if (!Q.Line[3]) {DrawLine(Q.arr[3], Q.arr[0]); Q.Line[3] = true;}

        }
        if (CurN[1][2] < 0) {
            if (!Q.Line[2]) { DrawLine(Q.arr[3], Q.arr[2]); Q.Line[2] = true;}

            if (!Q.Line[9]) { DrawLine(Q.arr[2], Q.arr[6]); Q.Line[9] = true;}

            if (!Q.Line[6]) { DrawLine(Q.arr[6], Q.arr[7]); Q.Line[6] = true;}

            if (!Q.Line[10]) { DrawLine(Q.arr[7], Q.arr[3]); Q.Line[10] = true;}

        }
        if (CurN[2][2] < 0) {
            if (!Q.Line[4]) { DrawLine(Q.arr[4], Q.arr[5]); Q.Line[4] = true;}

            if (!Q.Line[5]) { DrawLine(Q.arr[5], Q.arr[6]); Q.Line[5] = true;}

            if (!Q.Line[6]) { DrawLine(Q.arr[6], Q.arr[7]); Q.Line[6] = true;}

            if (!Q.Line[7]) { DrawLine(Q.arr[7], Q.arr[4]); Q.Line[7] = true;}

        }
        if (CurN[3][2] < 0) {
            if (!Q.Line[4]) { DrawLine(Q.arr[4], Q.arr[5]); Q.Line[4] = true;}

            if (!Q.Line[8]) { DrawLine(Q.arr[5], Q.arr[1]); Q.Line[8] = true;}

            if (!Q.Line[0]) { DrawLine(Q.arr[1], Q.arr[0]); Q.Line[0] = true;}

            if (!Q.Line[11]) { DrawLine(Q.arr[0], Q.arr[4]); Q.Line[11] = true;}

        }
        if (CurN[4][2] < 0) {
            if (!Q.Line[11]) { DrawLine(Q.arr[4], Q.arr[0]); Q.Line[11] = true;}

            if (!Q.Line[3]) { DrawLine(Q.arr[0], Q.arr[3]); Q.Line[3] = true;}

            if (!Q.Line[10]) { DrawLine(Q.arr[3], Q.arr[7]); Q.Line[10] = true;}

            if (!Q.Line[7]) { DrawLine(Q.arr[7], Q.arr[4]); Q.Line[7] = true;}

        }
        if (CurN[5][2] < 0) {
            if (!Q.Line[8]) { DrawLine(Q.arr[1], Q.arr[5]); Q.Line[8] = true;}

            if (!Q.Line[5]) { DrawLine(Q.arr[5], Q.arr[6]); Q.Line[5] = true;}

            if (!Q.Line[9]) { DrawLine(Q.arr[6], Q.arr[2]); Q.Line[9] = true;}

            if (!Q.Line[1]) { DrawLine(Q.arr[2], Q.arr[1]); Q.Line[1] = true;}

        }
        Q.Line = new boolean[]{false, false, false, false, false, false, false, false, false, false, false, false};
        Q.LastPx = 0;
    };



    public static void Loop() {
        if (Testing.LoopCount > Testing.LoopMax) {
            return;
        }
        String TestingStr = "" + Testing.LoopCount;
        while (TestingStr.length() < 3) {
            TestingStr = "0" + TestingStr;
        }
        Double[][] MTrans =Translate.apply(I, -Q.arr[8].V[0], -Q.arr[8].V[1], -Q.arr[8].V[2]);
        MTrans = RotateX.apply(MTrans, 1.0);
        MTrans = RotateY.apply(MTrans, 3.0);
        MTrans = RotateZ.apply(MTrans, 5.0);
        MTrans = Translate.apply(MTrans, Q.arr[8].V[0], Q.arr[8].V[1], Q.arr[8].V[2]);
        MQube = MMulti.apply(MTrans, MQube);
        int i = 8;
        for (; i > -1; i--) {
            Q.arr[i].V = VMulti.apply(MTrans, Q.arr[i].V);
        }
        DrawQube.apply();
        Testing.LoopCount++;
        Loop();
    }

    public static VoidIParmSupplier<Double> Init = (Double CubeSize) -> {
        // init/reset vars
        Origin = new Double[]{150.0, 150.0, 20.0, 1.0};
        Testing.LoopCount = 0;
        Testing.LoopMax = 50;

        MTrans = new Double[][]{
                {1.0, 0.0, 0.0, 0.0},
                {0.0, 1.0, 0.0, 0.0},
                {0.0, 0.0, 1.0, 0.0},
                {0.0, 0.0, 0.0, 1.0}
        }; // transformation matrix

        MQube = new Double[][]{
                {1.0, 0.0, 0.0, 0.0},
                {0.0, 1.0, 0.0, 0.0},
                {0.0, 0.0, 1.0, 0.0},
                {0.0, 0.0, 0.0, 1.0}
        }; // position information of qube

        I = new Double[][]{
                {1.0, 0.0, 0.0, 0.0},
                {0.0, 1.0, 0.0, 0.0},
                {0.0, 0.0, 1.0, 0.0},
                {0.0, 0.0, 0.0, 1.0}
        }; // entity matrix


        // create qube
        Q.arr[0] = new Px(-CubeSize, -CubeSize, CubeSize);
        Q.arr[1] = new Px(-CubeSize, CubeSize, CubeSize);
        Q.arr[2] = new Px(CubeSize, CubeSize, CubeSize);
        Q.arr[3] = new Px(CubeSize, -CubeSize, CubeSize);
        Q.arr[4] = new Px(-CubeSize, -CubeSize, -CubeSize);
        Q.arr[5] = new Px(-CubeSize, CubeSize, -CubeSize);
        Q.arr[6] = new Px(CubeSize, CubeSize, -CubeSize);
        Q.arr[7] = new Px(CubeSize, -CubeSize, -CubeSize);

        // center of gravity
        Q.arr[8] = new Px(0.0, 0.0, 0.0);
        // anti-clockwise edge check
        Q.Edge = new int[][]{{0, 1, 2},{3, 2, 6},{7, 6, 5},{4, 5, 1},{4, 0, 3},{1, 5, 6}};

        // calculate squad normals
        Q.Normal = new Double[Q.Edge.length][];
        for (int i = 0; i < Q.Edge.length; i++)
            Q.Normal[i] = CalcNormal.apply(Q.arr[Q.Edge[i][0]].V, Q.arr[Q.Edge[i][1]].V, Q.arr[Q.Edge[i][2]].V);

        // line drawn ?
        Q.Line = new boolean[]{false, false, false, false, false, false, false, false, false, false, false, false};

        // create line pixels
        Q.NumPx = 9 * 2 * CubeSize;

        MTrans = Translate.apply(MTrans, Origin[0], Origin[1], Origin[2]);
        MQube = MMulti.apply(MTrans, MQube);


        for (int i = 0; i < 9; i++) {
            Q.arr[i].V = VMulti.apply(MTrans, Q.arr[i].V);
        }
        DrawQube.apply();
        Loop();

        // Perform a simple sum-based verification.
        double sum = 0;
        for (int i = 0; i < Q.arr.length; ++i) {
            Double[] vector = Q.arr[i].V;
            for (int j = 0; j < vector.length; ++j) {
                sum += vector[j];
            }
        }
        double expected = 0;
        if (CubeSize.intValue() ==  20) {
            expected = validation.getOrDefault(CubeSize.intValue(), 0.0);
        } else if (CubeSize.intValue() ==  40) {
            expected = validation.getOrDefault(CubeSize.intValue(), 0.0);
        } else if (CubeSize.intValue() ==  80) {
            expected = validation.getOrDefault(CubeSize.intValue(), 0.0);
        } else if (CubeSize.intValue() == 160) {
            expected = validation.getOrDefault(CubeSize.intValue(), 0.0);
        }
        if (sum != expected) {
            throw new Error("Error: bad vector sum for CubeSize = " + CubeSize + "; expected " + expected + " but got " + sum);
        }

//         System.out.println("vector sum for cubeSize = " + CubeSize.intValue() + "; expected " + expected + " but got " + sum);

    };
    public static void run() {
        int iter = 0;
        while (iter <= 10) {
            int i = 20;
            while (i <= 160) {
                Init.apply((double) i);
                i *= 2;
            }
            iter++;
        }
    }

// let runner = new BenchmarkRunner("Array Access - RunThreeDCube", RunThreeDCube);
// runner.run();

}
class Benchmark {
    public static void main(String[] args) {
        double start = System.nanoTime();
        ThreeDCube.run();
        double end = System.nanoTime();
        double time = (end - start) / 1000000;
        System.out.println("Array Access - RunThreeDCube:\t" + time + "\tms");
        
    }
}

@FunctionalInterface
interface Function<T, U, V, W, R> {
    R apply(T t, U u, V v, W w);
}

@FunctionalInterface
interface VoidINullSupplier {
    void apply();
}

@FunctionalInterface
interface VoidIParmSupplier<T> {
    void apply(T t);
}

@FunctionalInterface
interface DoubleFunction<T, U, R> {
    R apply(T t, U u);
}

@FunctionalInterface
interface ThreeDoubleIDoubleFunction<T, U, V, R> {
    R apply(T t, U u, V v);
}

